% 1. 加载点集矩阵（每行是一个点，列是x,y,z）
% fixed = LF_Point([1:3,5],:);
% moving = LS_Point([1:3,5],:);

% 原始点数据（单位为 mm，因为1.0e+03 * 是毫米级）
fixed = 1e3 * [
    0.3621    1.1949    0.0679;
    0.5801    0.8066    0.0767;
    0.3504    0.3339    0.0812;
    1.1551    0.0796    0.0826
];

moving = 1e3 * [
    0.3434    1.6961    0.0527;
    0.6164    1.1949    0.0549;
    0.3134    0.5879    0.0586;
    1.3381    0.2496    0.0575
];

% 构造增广矩阵
N = size(moving, 1);
moving_aug = [moving, ones(N,1)];  % N×4

% 解最小二乘问题：moving_aug * T ≈ fixed
T_affine = moving_aug \ fixed;     % 得到 4×3 的仿射变换矩阵

% 构造 4×4 齐次变换矩阵（列向量风格）
T_4x4 = eye(4);
T_4x4(1:3, 1:3) = T_affine(1:3, :)';   % A 部分，转置
T_4x4(1:3, 4)   = T_affine(4, :)';     % t 部分

% 应用变换到 moving 点
moving_aug_homo = [moving, ones(N,1)] * T_4x4';   % 注意右乘 T'（行向量）
moving_reg = moving_aug_homo(:,1:3);

% 误差计算
errors = sqrt(sum((fixed - moving_reg).^2, 2));
mean_err = mean(errors);
rms_err = sqrt(mean(errors.^2));

% 显示结果
fprintf('✅ 拟合完成：\n');
fprintf('平均误差 (um): %.4f\n', mean_err);
fprintf('RMS 误差   (um): %.4f\n', rms_err);
fprintf('仿射变换矩阵 T_4x4 = \n');
disp(T_4x4);


% 可视化结果
figure('Color','w','Position',[200 200 1000 500]);

% 原始点云 vs fixed
subplot(1,2,1);
scatter3(fixed(:,1), fixed(:,2), fixed(:,3), 100, 'r', 'filled'); hold on;
scatter3(moving(:,1), moving(:,2), moving(:,3), 100, 'b', 'o', 'LineWidth', 1.5);
legend('Fixed (●)', 'Moving (○)');
title('Before Registration');
xlabel('X (μm)'); ylabel('Y (μm)'); zlabel('Z (μm)');
axis equal; grid on;

% 配准后 vs fixed
subplot(1,2,2);
scatter3(fixed(:,1), fixed(:,2), fixed(:,3), 100, 'r', 'filled'); hold on;
scatter3(moving_reg(:,1), moving_reg(:,2), moving_reg(:,3), 100, 'g', 'o', 'LineWidth', 1.5);
legend('Fixed (●)', 'Registered (○)');
title('After Affine Registration');
xlabel('X (μm)'); ylabel('Y (μm)'); zlabel('Z (μm)');
axis equal; grid on;